//
// Created by 24087 on 2025/3/6.
//

#include "../Inc/Encoder.h"

#include "Control.h"

/*编码器原始数据*/
static volatile int64_t _Counter = 0; /*测量值*/
static volatile int8_t _Direction = 0; /*方向量*/
static GPIO_PinState _A, _B; /*相状态*/
static const int8_t _Encoder_State_Index[4] = {
 /*索引规则：[边沿类型][B相状态] --> (增量，方向)*/
 /*边沿类型：0==上升沿， 1==下降沿*/
 /*B相状态：0==低电平， 1==高电平*/
 +1, -1, -1, +1 /*分别对应 00, 01, 10, 11*/
};
static uint64_t _last_time = 0.0f, _current_time = 0.0f, _real_time = 0.0f;
/*
 * 中断回调函数，用来处理电机A相参数
 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
 /*首先读取A、B相的电压极性*/
 /*声明和赋值分开是为了最大程度保证原子性*/
 _A = HAL_GPIO_ReadPin(E1A_GPIO_Port, E1A_Pin);
 _B = HAL_GPIO_ReadPin(E1B_GPIO_Port, E1B_Pin);

 /*
  * 逻辑处理部分，采用索引表的形式来赋值
  * 分别处理编码器计数值、电机运行方向值、T法测速中周期值
  */

 /*根据四种不同的情况生成索引*/
 const int8_t edge = (_A == GPIO_PIN_RESET) ? 1 : 0;
 const int8_t B_State = (_B == GPIO_PIN_RESET) ? 0 : 1;
 const uint8_t index = (edge << 1) | B_State;

 /*查表更新计数值、方向值和周期值*/
 const int8_t index_value = _Encoder_State_Index[index];
 _Counter += index_value;
 _Direction = (index_value > 0) ? +1 : -1;
 _last_time = _current_time;
 _current_time = Get_us();
}

int64_t Encoder_GetValue()
{
 return _Counter;
}

double Encoder_GetAngle()
{
 return 360.0f * 300.0f / (22.0f * 2783.0f) * _Counter;
}

double Encoder_GetSpeed()
{
 return _Direction / (Encoder_TimeSelect() * 1e-6f) / 22.0 / (2783.0 / 300.0) * 360.0;
}

double Encoder_TimeSelect()
{
 _real_time = Get_us();
 return _real_time - _current_time >= _current_time - _last_time ? _real_time - _current_time : _current_time - _last_time;
}

